//=======================================================
//AUTHOR: JEFF XL. ZHANG ARIZONA STATE UNIVERSITY
//ALL RIGHTS RESERVED  2008
//=======================================================
#pragma once
//#ifndef		//potential replacement for pragma once
#include <cv.h>
#include <cxcore.h>
#include <highgui.h>
#include <math.h>
#include <vector>
#include <stdio.h>
#include <iostream>
#include "line.h"
#include "display.h"
#include "data_structure.h"
#include "interpolation.h"


using namespace std;

bool PRINT_MUTUAL_MAT = 0;	
bool PRINT_SIMILARITY_MAT = 0;	
bool PRINT_MATCHING_LIST = 0;	

class FeatureComparison{
public:
	FeatureComparison();
	void compare(const ImageFeature &feature1, const ImageFeature &feature2);
	vector<double> FeatureComparison::compute_hist_centriods(ImageFeature feature) const;
	void print_similarity_matrix(CvMat* Smatrix,int rows,int cols) const;
//	CvMat* compute_centroid_distances(vector<double> centroid1, vector<double> centroid2)const;
	void FeatureComparison::compute_centroid_distances(CvMat *dist_cents, vector<double> centroid1, vector<double> centroid2)const;
	void compute_diff_shift(CvMat* dist_cent,int m,int n,CvMat* similarity_matrix,int k,int j,int i,ImageFeature frame_anchor,ImageFeature frame_target)const;
	vector<IndexPair> mutual_match;

private:
	double similarity_index;
};

FeatureComparison::FeatureComparison(){
	similarity_index = .0;
}

//COMPUTE SIMILARITY GIVEN ALL THE CONDITIONS
void FeatureComparison::compute_diff_shift(CvMat* dist_cent,int m,int n,CvMat* similarity_matrix,int k,int j,int i,ImageFeature frame_anchor,ImageFeature frame_target)const {
	double buffer_1 = .0, buffer_2 = .0, buffer_a = .0;
	if(i < abs(cvmGet(dist_cent,m,n)))		//LEFT TAIL
	//COMPUTE THE HEAD
	//DIST IS ANCHOR - TARGET
	//SIMILARITY[m][n] += ((DIST>0)?(FRAME_ANCHOR.HIST[M].VALUE_RGB[K][J][I]):(FRAME_TARGET.HIST[N].VALUE_RGB[K][J][I]))^2;
		cvmSet(similarity_matrix,m,n,cvmGet(similarity_matrix,m,n) + pow(((cvmGet(dist_cent,m,n)>0)?frame_anchor.Hists[m].value_rgb[k][j][i]:frame_target.Hists[n].value_rgb[k][j][i]),2.0));	
	else
		if(i > frame_anchor.Hists[m].get_dimension3()){			//RIGHT TAIL
			//SIMILARITY[m][n] += ((DIST>0)?(FRAME_TARGET.HIST[N].VALUE_RGB[K][J][I]):(FRAME_ANCHOR.T[M].VALUE_RGB[K][J][I]))^2;
			buffer_1 = cvmGet(similarity_matrix,m,n);
			buffer_2 = pow(((cvmGet(dist_cent,m,n)>0)?frame_target.Hists[n].value_rgb[k][j][i]:frame_anchor.Hists[m].value_rgb[k][j][i]),2.0);
			buffer_a = buffer_1 + buffer_2;
			cvmSet(similarity_matrix,m,n,buffer_a);	
			}
		else		//MAIN BODY
			cvmSet(similarity_matrix,m,n,cvmGet(similarity_matrix,m,n) + pow((frame_anchor.Hists[m].value_rgb[k][j][i] - frame_target.Hists[n].value_rgb[k][j][i]),2.0));	

}

//COMPUTE SIMILARITY MATRIX FOR HISTOGRAM
void FeatureComparison::compare(const ImageFeature &frame_anchor, const ImageFeature &frame_target)  {
	vector<double> centroid1 = compute_hist_centriods(frame_anchor);
	vector<double> centroid2 = compute_hist_centriods(frame_target);
	double threshold_slope_diff = CV_PI/10; // PI/10	********ARBITRARILY DEFINED************
	double threshold_centroid_distance = 30;	// 30 pixels  ********ARBITRARILY DEFINED************
	CvMat *dist_cent = cvCreateMat(centroid1.size(),centroid2.size(),CV_64FC1);
//	dist_cent = compute_centroid_distances(centroid1,centroid2);		//DISTANCE BETWEEN TWO group of CENTROIDS
	CvPoint centroid_anchor, centroid_target;
	compute_centroid_distances(dist_cent,centroid1,centroid2);
	//DEBUGGING PRINTOUT
//	cout<<"CvMat *dist_cent[0][0] = "<<cvmGet(dist_cent,0,0)<<endl;		//anchor - target

	int rows = frame_anchor.Hists.size();
	int cols = frame_target.Hists.size();
//	int cols = frame_target.get_lines().size();		//OLD WAY
	double similarity_index_base_sq = 256.0*pow(255.0,2.0);
	double similarity_index_base = 255.0;
	double difference_buffer = 0;
	CvMat *similarity_matrix = cvCreateMat(rows,cols,CV_64FC1);  //M*N matrix
//	CvMat similarity_matrix = cvCreateMat(frame_anchor.get_lines().size(),frame_target.get_lines().size(),CV_64FC1);  //M*N matrix	

	//PROPER LOOP YET TO BE DEFINED
	if(PRINT_MUTUAL_MAT) {cout<<"Distance Matrix:"<<endl<<"	  ";
	for(int n = 0; n<cols; n++) cout<<n<<"  ";			cout<<endl;}
	for(int m = 0; m < rows; m++){		//FOR EACH LINE IN ANCHOR(0)
		if(PRINT_MUTUAL_MAT) cout<<m<<"	";
		for(int n = 0; n < cols; n++){		//FOR EACH LINE IN TARGET(1)
			cvmSet(similarity_matrix,m,n,0.0);
			//similarity_matrix[i][j] = acomparison.compare();
			for(int k = 2; k < frame_anchor.Hists[0].get_dimension1(); k++){		//LOOP AMONG THREE SIDES (SIDE-A -> SIDE-B -> SIDE-C)
				for(int j = 0; j < frame_anchor.Hists[0].get_dimension2(); j++){		//LOOP AMONG THREE(SIX) CHANNELS 
					for(int i = 0; i < (frame_anchor.Hists[0].get_dimension3() + cvmGet(dist_cent,m,n)); i ++){	//LOOP FOR SIZE + DELTA TIMES
//						compute_diff_shift(dist_cent,m,n,similarity_matrix,k,j,i,frame_anchor,frame_target);
						//CONSIDERING NORMALIZATION BEFORE CONSTRUCTING HISTOGRAM, WE CAN ADOPT THIS SIMPLE SCHEME
//						cvmSet(similarity_matrix,m,n,((double)(abs(frame_anchor.Hists[m].value_rgb[k][j][i] - frame_target.Hists[n].value_rgb[k][j][i])))/256.0);
//						cvmSet(similarity_matrix,m,n,abs(frame_anchor.Hists[m].value_rgb[k][j][i] - frame_target.Hists[n].value_rgb[k][j][i]));
//						cout<<"i="<<i<<" j="<<j<<" k="<<k<<" similarity_matrix["<<m<<"]["<<n<<"]="<<abs(frame_anchor.Hists[m].value_rgb[k][j][i] - frame_target.Hists[n].value_rgb[k][j][i])<<endl;
						difference_buffer += abs(frame_anchor.Hists[m].value_rgb[k][j][i] - frame_target.Hists[n].value_rgb[k][j][i])/similarity_index_base;
					}
				}
			}	
			if(PRINT_MUTUAL_MAT) cout<<" "<<(int)difference_buffer;
		cvmSet(similarity_matrix,m,n,difference_buffer);
		difference_buffer = 0;
		}
		if(PRINT_MUTUAL_MAT) cout<<endl;
	}
	//TEST:OUTPUT SINGLE HISTOGRAM COMPARISON RESULT-SINGLE VALUE
	if(PRINT_MUTUAL_MAT)  cout<<"DI-SIMILARITY = "<<(double)cvmGet(similarity_matrix,0,0)/(3*similarity_index_base)<<endl;
	
	//FOR EACH LINE GET A MATCHING VECTOR
	vector<int> matching_list_anchor,matching_list_target;
	double min,value;
	int	index;
	if(PRINT_MATCHING_LIST)  cout<<"Anchor: "<<endl;
	for(int m = 0; m < rows; m++){		//FOR EACH LINE IN ANCHOR(0)
//		min = cvmGet(similarity_matrix,m,0);		//BUG USING 0 AS INITIAL VALUE
		min = similarity_index_base;
		index = 0;		//PROBLEM: ALWAYS DEFAULT TO BE ZERO--->SHOULD DEFAULT TO BE NULL
		for(int n = 0; n < cols; n++){		//FOR EACH LINE IN TARGET(1)
			centroid_anchor = frame_anchor.get_lines()[m].get_cent();
			centroid_target = frame_target.get_lines()[n].get_cent();
			value = cvmGet(similarity_matrix,m,n);
			if (abs(atan(frame_anchor.get_lines()[m].report_slope()) - atan(frame_target.get_lines()[n].report_slope())) < threshold_slope_diff){	//SLOPE
				if( PointDistance(centroid_anchor,centroid_target) < threshold_centroid_distance ){		//CENTOID
					if(value<min){
						min = value;
						index = n;
					}			
				}
			}
		}
		matching_list_anchor.push_back(index);
		if(PRINT_MATCHING_LIST) cout<<matching_list_anchor[m]<<"	";
	}
	if(PRINT_MATCHING_LIST) cout<<endl<<"Target: "<<endl;

	for (int n = 0; n < cols; n++)	//FOR EACH LINE IN TARGET(1)
	{
//		min = cvmGet(similarity_matrix,0,n);		//BUG USING 0 AS INITIAL VALUE
		min = similarity_index_base;
		index = 0;
		for (int m = 0; m < rows; m++)	//FOR EACH LINE IN ANCHOR(0)
		{
			//IF SLOPE AND LOCATION ADJACENT
			centroid_anchor = frame_anchor.get_lines()[m].get_cent();
			centroid_target = frame_target.get_lines()[n].get_cent();
			value = cvmGet(similarity_matrix,m,n);
			if (abs(atan(frame_anchor.get_lines()[m].report_slope()) - atan(frame_target.get_lines()[n].report_slope())) < threshold_slope_diff){	//SLOPE
				if( PointDistance(centroid_anchor,centroid_target) < threshold_centroid_distance ){		//CENTOID
					if(value<min){	//FIND MINIMUM
						min = value;
						index = m;
					}
				}	
			}
		}		
		matching_list_target.push_back(index);
		if(PRINT_MATCHING_LIST) cout<<matching_list_target[n]<<"	";	
	}
	if(PRINT_MATCHING_LIST) cout<<endl;

	//MUTURAL/SINGLUAR MATCH
	bool anchor_is_longer = (matching_list_anchor.size() > matching_list_target.size());
//	vector<IndexPair> mutual_match;
	IndexPair index_pair_buffer;
	CvFont textfont;
	cvInitFont(&textfont,CV_FONT_HERSHEY_COMPLEX_SMALL,0.7f,0.7f,.0,1,CV_AA);
//	CvPoint centroid_anchor, centroid_target;

	for (int i = 0; i < (anchor_is_longer?matching_list_target.size():matching_list_anchor.size()); i++)
	{
		if(PRINT_MUTUAL_MAT) cout<<"The "<<i<<"th line in "<<(anchor_is_longer?"target":"anchor")<< " <--->";
		if (anchor_is_longer){
			if (matching_list_anchor[matching_list_target[i]] == i){	//HISTOGRAM SIMILARITY
				if(PRINT_MUTUAL_MAT) cout<<matching_list_target[i]<<"th line in "<<"anchor";
				index_pair_buffer.set_index(matching_list_target[i],i);
				mutual_match.push_back(index_pair_buffer);
			}
		}
		else	//TARGET IS LONGER
			if (matching_list_target[matching_list_anchor[i]] == i){
				if(PRINT_MUTUAL_MAT) cout<<matching_list_anchor[i]<<"th line in "<<"target";
				index_pair_buffer.set_index(i,matching_list_anchor[i]);
				mutual_match.push_back(index_pair_buffer);
			}
		if(PRINT_MUTUAL_MAT) cout<<endl;
}

	if(PRINT_MUTUAL_MAT){
		if(!mutual_match.empty())
			for (int i = 0;i<mutual_match.size() ; i++)
			{
				cout<<"Mutual Match "<< i << " ("<<mutual_match[i].get_index1()<<","<<mutual_match[i].get_index2()<<")."<<endl;		
			}
	}

	//VISUALIZE
	IplImage *matched_anchor = frame_anchor.get_image();
	IplImage *matched_target = frame_target.get_image();
	char *i_string_anchor,*i_string_target;
	for (int i = 0; i < mutual_match.size(); i++)
	{
		cvLine(matched_anchor,frame_anchor.get_lines()[mutual_match[i].get_index1()].Pt1,frame_anchor.get_lines()[mutual_match[i].get_index1()].Pt2,cvScalar(0,0,255,0),2,8,0);
		cvLine(matched_target,frame_target.get_lines()[mutual_match[i].get_index2()].Pt1,frame_target.get_lines()[mutual_match[i].get_index2()].Pt2,cvScalar(0,0,255,0),2,8,0);
//		i_string_anchor = new char[log((double)i)/log((double)10) + 1];
//		i_string_target = new char[log((double)i)/log((double)10) + 1];
		i_string_anchor = new char[log10((double)i) + 1];
		i_string_target = new char[log10((double)i) + 1];
		sprintf(i_string_anchor,"%d-%d",i,mutual_match[i].get_index1());		//MATCHING_ID-REAL_ID
		sprintf(i_string_target,"%d-%d",i,mutual_match[i].get_index2());		//MATCHING_ID-REAL_ID
		cvPutText(matched_anchor,i_string_anchor,frame_anchor.get_lines()[mutual_match[i].get_index1()].get_cent(),&textfont,cvScalar(255.0,.0,.0,.0));
		cvPutText(matched_target,i_string_target,frame_target.get_lines()[mutual_match[i].get_index2()].get_cent(),&textfont,cvScalar(255.0,.0,.0,.0));
	}
	Display("matched_anchor",matched_anchor);
	Display("matched_target",matched_target);	

//	return score;

//	print_similarity_matrix(similarity_matrix,similarity_matrix->rows,similarity_matrix->cols);
//	return 0;
}

//COMPUTE HISTOGRAM CENTROIDS OF AN IMAGE FEATURE OUTPUT A VECTOR OF CENTROIDS IN DOUBLE
vector<double> FeatureComparison::compute_hist_centriods(ImageFeature feature) const{
	vector<double> centroid;
	double centroid_buffer,weighted_sum, sum;
	for (int i = 0; i < feature.Hists.size(); i++)		//FOR EACH LINE
	{
		weighted_sum = 0; sum = 0;
		for (int j = 0; j < feature.Hists[0].get_dimension3(); j++)
		{
			weighted_sum += j*feature.Hists[i].value_rgb[2][0][j];		//BLUE CHANNEL ONLY, RG TO BE IMPLEMENTED
			sum += feature.Hists[i].value_rgb[2][0][j];
		}
		//APPLY FORMULA HERE
		centroid_buffer = weighted_sum/sum;
		centroid.push_back(centroid_buffer);		
	}
	return centroid;
//	return feature.Hists.size()/2;			//HUGE BUG!!!!!!!!!!!!!!!!!!!!!!!!!!!
}


void FeatureComparison::print_similarity_matrix(CvMat* Smatrix, int rows, int cols) const{
	for(int i = 0; i < rows; i++)
		for(int j = 0; j < cols; j++){
			cout<<cvmGet(Smatrix,i,j)<<" ";
		}
		cout<<endl;
}

//DISTANCE BETWEEN TWO group of CENTROIDS
void FeatureComparison::compute_centroid_distances(CvMat*dist_cents, vector<double> centroid1, vector<double> centroid2)const {
	int m = centroid1.size();
	int n = centroid2.size();
//	CvMat* dist_cents = cvCreateMat(m,n,CV_64FC1);
	for (int i = 0; i < m; i++)
	{
		for (int j = 0; j < n; j++)
		{
			cvmSet(dist_cents,i,j,centroid1[i] - centroid2[j]);
		}
		
	}
//	return dist_cents;
}

